package pathAlgorithms;
import javax.vecmath.Point2d;

import program.RoboMap;

public class WeightedRoboGraph extends RoboGraph {
	
	public WeightedRoboGraph(RoboMap map) {
		super(map);
	}

	private RoboNode startPos;
	public static Point2d GLOBAL_START_POS;
	
	@Override
	protected RoboNode addNodeIfValid(double x, double y, RoboPath path, RoboNode goalNode) {
		if(x > xSize || y > ySize || x < -xSize || y < -ySize) {
			return null;
		}
		
		Point2d point = new Point2d(x,y);
		RoboMap.Content content = roboMap.getContentFromPosition(point);
		RoboNode node = new RoboNode(point,content);
		RoboNode globalStartNode = new RoboNode (GLOBAL_START_POS, RoboMap.Content.CLEAR);
		graphLogger.nodesCreated++;
		
		double fromGlobalStartToCurrent = distance (globalStartNode, node);
		double fromCurrentToGoal = distance (node, goalNode);
		double weight = 10 * (fromGlobalStartToCurrent / (fromGlobalStartToCurrent + fromCurrentToGoal));
		
		if(content != RoboMap.Content.OBJECT && !visited.contains(point)) {
			
			if(content == RoboMap.Content.MAYBEOBJECT) {
				weight *= maybeObjectFactor;
			}
			

			RoboPath newPath = new RoboPath(path,node, weight * fromCurrentToGoal);
			graphLogger.pathsCreated++;
			
			queue.add(newPath);
			graphLogger.pathsAddedToQue++;
			
			return node;
		}else {
			return null;
		}
	}
	
	@Override 
	public Point2d[] findPath(RoboNode startNode, RoboNode goalNode) {
		this.startPos = startNode;
		return super.findPath(startNode, goalNode);
	}
}
